/*
 * Position.cpp
 *
 *  Created on: May 7, 2013
 *      Author: Daniel
 */

#include "Position.h"
#include "math.h"

#include <iostream>
#include <sstream>

/**
 * Default constructor
 */
Position::Position()
{
	x = xold = 0;
	z = zold= 0;
}

/**
 * Preferred constructor
 */
Position::Position(double x, double z)
{
	this->x = xold = x;
	this->z = zold = z;
}

/**
 * Default destructor
 */
Position::~Position()
{
}

double Position::getX()
{
	return x;
}

double Position::getZ()
{
	return z;
}

void Position::setX(double x)
{
	this->x = x;
}

void Position::setZ(double z)
{
	this->z = z;
}

void Position::addX(double delta)
{
	this->x += delta;
}

void Position::addZ(double delta)
{
	this->z += delta;
}

/**
 * Add other Position vector to this vector
 */
void Position::add(Position *pos)
{
	addX(pos->getX());
	addZ(pos->getZ());
}

/**
 * Subtract other Position vector from this vector
 */
void Position::sub(Position *pos)
{
	addX(-pos->getX());
	addZ(-pos->getZ());
}

/**
 * Calculate the magnitude of two Position vectors
 */
double Position::mag()
{
	double magnitude = sqrt(getX()*getX() + getZ()*getZ());
	return magnitude;
}

/**
 * Divide the Position vector by a scalar
 */
void Position::div(double scalar)
{
	setX(getX()/scalar);
	setZ(getZ()/scalar);
}

/**
 * Multiply the Position vector by a scalar
 */
void Position::mul(double scalar)
{
	setX(getX()*scalar);
	setZ(getZ()*scalar);
}

/**
 * Normalize the Position vector
 */
void Position::normalize()
{
	div(mag());
}

/**
 * Calculate the distance between two Positions
 */
double Position::distance(Position *pos)
{
	double deltaX 	= this->getX() - pos->getX();
	double deltaZ 	= this->getZ() - pos->getZ();
	double distance = sqrt(deltaX*deltaX + deltaZ*deltaZ);

	return distance;
}

/**
 * Subtract two Position vectors from each other
 */
Position Position::sub(Position *pos1, Position *pos2)
{
	double x = pos1->getX() - pos2->getX();
	double z = pos1->getZ() - pos2->getZ();
	return Position(x,z);
}

/**
 * Set the magnitude of the Position vector
 */
void Position::setMag(double mag)
{
	   normalize();
	   x *= mag;
	   z *= mag;
}

/**
 * Calculate the angle between this position and the origin (pos)
 */
double Position::angle(Position *pos)
{
	double deltaX 	= this->getX() - pos->getX();
	double deltaZ 	= this->getZ() - pos->getZ();
	double angle	= atan2(deltaZ, deltaX);

	return angle;
}

/**
 * Create a backup of the last position
 */
void Position::backup()
{
	xold = x;
	zold = z;
}

/**
 * Restore the backup of the last position
 */
void Position::restore()
{
	x = xold;
	z = zold;
}

/**
 * Create a string representation of this Position object
 */
std::string Position::toString()
{
	std::stringstream pos;
	pos << "<Position(" << x << "," << z << ")>";
	return pos.str();
}
